#include "stm32f1xx_hal.h"

#include <sys/time.h>
#include <inttypes.h>
#include "FreeRTOS.h"
#include <math.h>
#include <pid_pmeasure.h>
#include <time.h>
#include <stdlib.h>
#include <stdint.h>
#include <stdbool.h>
static long millis(void) {
	return HAL_GetTick();
}
static void initialize(pid_pmeasure_ctrl_t *handle) {
	handle->outputSum = *(handle->_output);
	handle->lastInput = *(handle->_input);
	if (handle->outputSum > handle->outMax)
		handle->outputSum = handle->outMax;
	else if (handle->outputSum < handle->outMin)
		handle->outputSum = handle->outMin;
}

pid_pmeasure_ctrl_t* pid_pmeasure_ctrl_new(float *input, float *output,
		float *setpoint, float kp, float ki, float kd, pid_measure_type pOn,
		pid_out_dir_type controllerDirection) {
	pid_pmeasure_ctrl_t *handle = (pid_pmeasure_ctrl_t*) malloc(
			sizeof(pid_pmeasure_ctrl_t));

	handle->_output = output;
	handle->_input = input;
	handle->_setpoint = setpoint;
	handle->inAuto = false;

	pid_pmeasure_ctrl_set_output_limits(handle, 0, 255); // default output limit corresponds to
														 // the arduino pwm limits

	handle->SampleTime = 100; // default Controller Sample Time is 0.1 seconds

	pid_pmeasure_ctrl_set_controller_direction(handle, controllerDirection);
	pid_pmeasure_ctrl_set_tunings(handle, kp, ki, kd, pOn);

	handle->lastTime = millis() - handle->SampleTime;
	return handle;
}
void pid_pmeasure_ctrl_delete(pid_pmeasure_ctrl_t *handle) {
	if (NULL != handle)
		free(handle);
}

void pid_pmeasure_ctrl_set_model(pid_pmeasure_ctrl_t *handle,
		pid_ctrl_model_type model) {
	bool newAuto = (model == AUTOMATIC);
	if (newAuto && !handle->inAuto) { /*we just went from manual to auto*/
		initialize(handle);
	}
	handle->inAuto = newAuto;
}
bool pid_pmeasure_ctrl_compute(pid_pmeasure_ctrl_t *handle) {
	if (!handle->inAuto)
		return false;
	unsigned long now = millis();
	unsigned long timeChange = (now - handle->lastTime);
	if (timeChange >= handle->SampleTime) {
		/*Compute all the working error variables*/
		float input = *(handle->_input);
		float error = *(handle->_setpoint) - input;
		float dInput = (input - handle->lastInput);
		handle->outputSum += (handle->ki * error);

		/*Add Proportional on Measurement, if P_ON_M is specified*/
		if (!handle->pOnE)
			handle->outputSum -= handle->kp * dInput;

		if (handle->outputSum > handle->outMax)
			handle->outputSum = handle->outMax;
		else if (handle->outputSum < handle->outMin)
			handle->outputSum = handle->outMin;

		/*Add Proportional on Error, if P_ON_E is specified*/
		float output;
		if (handle->pOnE)
			output = handle->kp * error;
		else
			output = 0;

		/*Compute Rest of PID Output*/
		output += handle->outputSum - handle->kd * dInput;

		if (output > handle->outMax)
			output = handle->outMax;
		else if (output < handle->outMin)
			output = handle->outMin;
		*(handle->_output) = output;

		/*Remember some variables for next time*/
		handle->lastInput = input;
		handle->lastTime = now;
		return true;
	} else
		return false;
}
void pid_pmeasure_ctrl_set_output_limits(pid_pmeasure_ctrl_t *handle, float Min,
		float Max) {
	if (Min >= Max)
		return;
	handle->outMin = Min;
	handle->outMax = Max;

	if (handle->inAuto) {
		if (*(handle->_output) > handle->outMax)
			*(handle->_output) = handle->outMax;
		else if (*(handle->_output) < handle->outMin)
			*(handle->_output) = handle->outMin;

		if (handle->outputSum > handle->outMax)
			handle->outputSum = handle->outMax;
		else if (handle->outputSum < handle->outMin)
			handle->outputSum = handle->outMin;
	}
}
void pid_pmeasure_ctrl_set_tunings(pid_pmeasure_ctrl_t *handle, float Kp,
		float Ki, float Kd, pid_measure_type POn) {
	if (Kp < 0 || Ki < 0 || Kd < 0)
		return;

	handle->pOn = POn;
	handle->pOnE = POn == P_ON_E;

	handle->dispKp = Kp;
	handle->dispKi = Ki;
	handle->dispKd = Kd;

	float SampleTimeInSec = ((float) handle->SampleTime) / 1000;
	handle->kp = Kp;
	handle->ki = Ki * SampleTimeInSec;
	handle->kd = Kd / SampleTimeInSec;

	if (handle->controllerDirection == REVERSE) {
		handle->kp = (0 - handle->kp);
		handle->ki = (0 - handle->ki);
		handle->kd = (0 - handle->kd);
	}
}
void pid_pmeasure_ctrl_set_controller_direction(pid_pmeasure_ctrl_t *handle,
		pid_out_dir_type dir) {
	if (handle->inAuto && dir != handle->controllerDirection) {
		handle->kp = (0 - handle->kp);
		handle->ki = (0 - handle->ki);
		handle->kd = (0 - handle->kd);
	}
	handle->controllerDirection = dir;
}
void pid_pmeasure_ctrl_set_sampletime(pid_pmeasure_ctrl_t *handle,
		int NewSampleTime) {
	if (NewSampleTime > 0) {
		float ratio = (float) NewSampleTime / (float) handle->SampleTime;
		handle->ki *= ratio;
		handle->kd /= ratio;
		handle->SampleTime = (unsigned long) NewSampleTime;
	}
}
float pid_pmeasure_ctrl_get_kp(pid_pmeasure_ctrl_t *handle) {
	return handle->dispKp;
}
float pid_pmeasure_ctrl_get_ki(pid_pmeasure_ctrl_t *handle) {
	return handle->dispKi;
}
float pid_pmeasure_ctrl_get_kd(pid_pmeasure_ctrl_t *handle) {
	return handle->dispKd;
}

pid_ctrl_model_type pid_pmeasure_ctrl_get_model(pid_pmeasure_ctrl_t *handle) {
	return handle->inAuto ? AUTOMATIC : MANUAL;
}
pid_out_dir_type pid_pmeasure_ctrl_get_direction(pid_pmeasure_ctrl_t *handle) {
	return handle->controllerDirection;
}
pid_measure_type pid_pmeasure_ctrl_get_measure_state(
		pid_pmeasure_ctrl_t *handle) {
	return handle->pOnE ? P_ON_E : P_ON_M;
}
